home *** CD-ROM | disk | FTP | other *** search
/ Amiga Tools 3 / Amiga Tools 3.iso / grafik / raytracing / rayshade-4.0.6.3 / libray / libobj / findroot.c < prev    next >
Encoding:
C/C++ Source or Header  |  1994-08-09  |  12.6 KB  |  455 lines
  1. /*
  2.  * findroot.c
  3.  *
  4.  * Copyright (C) 1989, 1991, Mark Podlipec, Craig E. Kolb
  5.  * All rights reserved.
  6.  *
  7.  * This software may be freely copied, modified, and redistributed
  8.  * provided that this copyright notice is preserved on all copies.
  9.  *
  10.  * You may not distribute this software, in whole or in part, as part of
  11.  * any commercial product without the express consent of the authors.
  12.  *
  13.  * There is no warranty or other guarantee of fitness of this software
  14.  * for any purpose.  It is provided solely "as is".
  15.  *
  16.  * findroot.c,v 4.1 1994/08/09 07:58:38 explorer Exp
  17.  *
  18.  * findroot.c,v
  19.  * Revision 4.1  1994/08/09  07:58:38  explorer
  20.  * Bump version to 4.1
  21.  *
  22.  * Revision 1.1.1.1  1994/08/08  04:52:08  explorer
  23.  * Initial import.  This is a prerelease of 4.0.6enh3, or 4.1 possibly.
  24.  *
  25.  * 4.1
  26.  * Initial version.
  27.  *
  28.  */
  29.  
  30. #include "libcommon/common.h"
  31.  
  32. /* 
  33.  * POLY_SIZE should be set equal to or larger than the highest degree
  34.  * polynomial to be solved by this routine. This structure could've
  35.  * been an argument to FindRoot, thus making it more flexible, but
  36.  * I decided it would be easier just to have it here and not have to worry
  37.  * about it in all the routines that call it.
  38.  *
  39.  * Some space optimization could be done, but I didn't think the savings
  40.  * would be worth the effort.
  41.  *
  42.  */
  43. #define POLY_SIZE 8
  44.  
  45. typedef struct STRUCT_POLY_STACK
  46. {
  47.  Float roots[POLY_SIZE];
  48.  int    valid[POLY_SIZE];
  49.  Float f[POLY_SIZE];
  50. } POLY_STACK;
  51.  
  52. static POLY_STACK gg[POLY_SIZE];
  53.  
  54. static int r_loop=0;
  55.  
  56. #define TRUE      1
  57. #define FALSE     0
  58. #define VALID     1
  59.  
  60. /*
  61. #define    FR_EPS     1e-8
  62. #define    FR_EPS_2   1e-9 
  63. */
  64. #define    FR_EPS     1e-7
  65. #define    FR_EPS_2   1e-8
  66. #define    FR_EPS_3   1e-24
  67. #define    FRIsZero(x)   ( ((x) > -FR_EPS)   && ((x) < FR_EPS)   )
  68. #define    FRIsZero3(x)  ( ((x) > -FR_EPS_3) && ((x) < FR_EPS_3) )
  69.  
  70. /*
  71.  * This is used to determine the sign of x
  72.  */
  73. #define FR_SGN(x)  ( ((x)<0.0)?-1:(((x)>0.0)?1:0) )
  74.  
  75. /*
  76.  * Evaluate the degree _Deg polynomial _p at point _x and put
  77.  * the value into _y(note: when calling give _y not &_y)
  78.  */
  79. #define FR_EVALPOLY(_p,_Deg,_x,_y) \
  80.  { int _i; \
  81.    _y = (_p)[(_Deg)]; \
  82.    for(_i=((_Deg)-1);_i>=0;_i--) _y=(_x)*_y + (_p)[_i]; \
  83.  }
  84.  
  85. #ifndef AMIGA
  86. Float sqrt();
  87. #endif
  88.  
  89. /*
  90.  * This is a specialized routine to find the smallest root in the
  91.  * interval a,b. FindRoot will return a 1 if a root is found and
  92.  * a 0 if no roots are found. If a root is found, it's value will
  93.  * be put in *root.
  94.  */ 
  95. int FindRoot(ff,degree,a,b,root,flag)
  96. Float *ff;        /* array of coefficients of the polynomial */
  97.             /* ex: 5x^2 - 2x + 3.0   ff[0] =  3.0
  98.                          ff[1] = -2.0
  99.                          ff[2] =  5.0  */
  100. int degree;        /* the degree of polynomial */
  101. Float a,b;        /* the interval to look for roots in */
  102. Float *root;        /* where to put a root if found */
  103. int flag;        /* Should always be -1 when called. This is
  104.                used to initialize derivatives the 1st
  105.                time called but not when FindRoot calls
  106.                itself subsequently.*/
  107. {
  108.     int i,loop;
  109.     Float *f1,*f0;
  110.  
  111. /*
  112.  * Just in case something really goes wrong bomb out before stack space 
  113.  * gets recursivley eaten away. (This happened during initial debug and
  114.  * I can't think a situation that would cause it now but it doesn't hurt.
  115.  *
  116.  * r_loop should be larger than maximum degree squared.
  117.  */
  118.  if (r_loop > (POLY_SIZE * (POLY_SIZE+1)) )
  119.  {
  120.   fprintf(stderr,"FindRoot Error: r_loop=%ld degree=%ld flag=%ld\n",
  121.      r_loop,degree,flag);
  122.   exit(0);
  123.  }
  124.  
  125.     /* not a valid polynomial as far as we're concerned. */
  126.     if (degree==0) return FALSE;
  127.  
  128.     /* Some useful variables. f0=ff() and f1=ff'(). */
  129.     f0 = gg[degree].f;
  130.     f1 = gg[degree-1].f;
  131.  
  132.     /* 
  133.       * Find Root called for the first time.
  134.       * Calculate all derivatives of the polynomial.
  135.       * and initialize static structure to hold them.
  136.       */
  137.     if (flag==-1)
  138.     {
  139.          register int i,j;
  140.  
  141.         /* Initialize valid flags to !VALID */
  142.         for(i=0;i<8;i++) for(j=0;j<8;j++) gg[i].valid[j]=0;
  143.  
  144.         /* Initialize and calculate coefficients for all 
  145.          * derivatives of ff() 
  146.          */
  147.         for(i=0;i<=degree;i++) f0[i] = ff[i];
  148.         for(i=degree-1;i>=2;i--) 
  149.             for(j=0;j<=i;j++) 
  150.                 gg[i].f[j] = (Float)(j+1) * gg[i+1].f[j+1];
  151.         flag = degree;
  152.         /* initialize variable that flags runaway recursion. Again
  153.          * I don't believe it will occur, but I won't rule it out.
  154.          */
  155.         r_loop=1;
  156.     } 
  157.      else
  158.     /* 
  159.       * Not the 1st time through. Check to see if we've already calculated
  160.       * a valid root for this derivative in this interval from a previous
  161.      * recursive call.
  162.       */
  163.     {
  164.         register int ix;
  165.         r_loop++;
  166.         ix = 0;
  167.  
  168.         /* Check the valid flags and if valid, check to see if it
  169.          * is in the current interval
  170.          */
  171.         while( (gg[degree].valid[ix]==VALID) )
  172.         {
  173.             if (   (gg[degree].roots[ix] > a )
  174.                 && (gg[degree].roots[ix] < b )          ) 
  175.             {
  176.                 *root = gg[degree].roots[ix];
  177.                 return TRUE;
  178.             }
  179.             ix++;
  180.             /* 
  181.              * If all possible roots at this degree have been 
  182.              * found and none of them are in the interval [a,b] 
  183.              * then return FALSE.
  184.               */
  185.             if (ix >= degree) return FALSE; 
  186.         } /* end of search for valid roots */
  187.     } /* end of not first time that FindRoot was called */
  188.  
  189.     /*
  190.      * If the leading coeff is very close to zero, approximate by reducing
  191.      * the degree and recalling FindRoot. Prevents explosions.
  192.      *
  193.      * blob.c currently could do this since it constructs polynomials from
  194.      * sums of other polynomials and not necessarily end up with degree 4.
  195.      *
  196.      * Check to see if this is ever called. TEST
  197.      */
  198.  
  199.      if ( FRIsZero3(gg[degree].f[degree]) ) 
  200.                     return( FindRoot(f0,(degree-1),a,b,root,flag) );
  201.  
  202.     /******************************************************************
  203.      * DEGREE==1
  204.      *
  205.      * If ff() is of degree 1 then it's a simple line.
  206.      */
  207.     if (degree==1)
  208.     {
  209.         if ( FRIsZero(f0[1]) ) return FALSE;
  210.         else *root = -f0[0]/f0[1];
  211.  
  212.         gg[degree].valid[0] = 1; 
  213.         gg[degree].roots[0] = *root; 
  214.         if ( (*root > a) && (*root < b) ) return TRUE;
  215.         else return FALSE;
  216.     } 
  217.     /******************************************************************
  218.      * DEGREE==2
  219.      *
  220.      * If ff() is of degree 2 then use Quadratic Equation to find roots.
  221.      */
  222.     if (degree==2)
  223.     {
  224.         register Float t,d0,d1;
  225.  
  226.         d0 = f0[1];
  227.         t = d0*d0 - 4.0 * f0[2]*f0[0];
  228.  
  229.         /* Return FALSE if roots are imaginary. */
  230.         if (FR_SGN(t) == -1) return FALSE; 
  231.  
  232.         /* There is only one root. */
  233.         if (FR_SGN(t) == 0.0) 
  234.         { 
  235.                 /* Note f0[2] was checked for zero above 
  236.                  * as gg[degree].f[degree].
  237.                  */
  238.                 d1 = -d0/(2.0 * f0[2]); 
  239.  
  240.                 /* Since for this set of coeff we found 
  241.                  * all possible roots, set up static structure 
  242.                  * to indicate this.
  243.                  */
  244.                 gg[degree].valid[0] = VALID;
  245.                 gg[degree].valid[1] = VALID;
  246.                 gg[degree].roots[0] = d1; 
  247.                 gg[degree].roots[1] = d1; 
  248.                 if ( (d1>a) && (d1<b))
  249.                 {
  250.                     *root = d1;
  251.                     return TRUE; 
  252.                 } 
  253.                 else return FALSE;
  254.         }
  255.  
  256.         /* FR_SGN(t) > 0  which means there are two roots */
  257.         t = sqrt(t);
  258.  
  259.         /* If the smallest root has already been found, then it 
  260.          * was not in the current interval [a,b]. So skip this
  261.          * and calculate the larger quadratic root.
  262.          * NOTE: if both of the roots are valid then we wouldn't
  263.          * be here, we would've either returned which ever one was
  264.          * the smallest in the interval [a,b] or returned false.
  265.          */
  266.         if (gg[degree].valid[0]!=VALID) 
  267.             {
  268.             /* Calculate smallest Quadratic root */
  269.             if (f0[2]>0.0) d1= (-d0-t)/(2.0*f0[2]);
  270.             else d1= (-d0+t)/(2.0*f0[2]);
  271.  
  272.             /* put it in the static structure */
  273.             gg[degree].valid[0] = VALID;
  274.             gg[degree].roots[0] = d1; 
  275.             if ((d1>a) && (d1<b))
  276.             { 
  277.                 *root = d1; 
  278.                 return TRUE; 
  279.             } /* fall through otherwise and calculate larger */
  280.         } /* fall through otherwise and calculate larger */
  281.     
  282.         /* Calculate largest Quadratic root */
  283.         if (f0[2]>0.0) d1= (-d0+t)/(2.0*f0[2]);
  284.         else d1= (-d0-t)/(2.0*f0[2]);
  285.  
  286.         /* put it in the static structure */
  287.         gg[degree].valid[1] = VALID;
  288.         gg[degree].roots[1] = d1; 
  289.         if ((d1>a) && (d1<b))
  290.         {
  291.             *root = d1;
  292.             return TRUE; 
  293.         }
  294.         else return FALSE;
  295.     } /* end of degree==2 */
  296.  
  297.      /******************************************************************
  298.       * DEGREE>=3
  299.       *
  300.       * Now for the fun stuff.
  301.       *
  302.       * The algorithm being find the smallest root of f'() in the interval
  303.       * [a,b]. This will either be a min or a max(we don't care which). Call
  304.       * this point c. Now evaluate f(a) and f(c). If they are of different
  305.       * signs, the f() has a root in the interal [a,c], use Newton's method
  306.       * to solve. If they are of the same sign, there is no root in [a,c]
  307.       * so move the interval in question to [c,b] and recall FindRoot.
  308.       * Oh, and if there is no min/max then the sign tests apply with a and b
  309.       * in the same way as the applied to a and c.
  310.       *
  311.       * There's some tricky parts, like what happens if there's a root at a?
  312.       * Since this is raytracing, we assume the ray was fired off from a 
  313.       * surface and we didn't quite clear it. So we walk a forward in
  314.       * increments of FR_EPS2 which is smaller than FR_EPS. If after an
  315.       * arbitrary amount, f(a) is still zero, then we return the root at a+.
  316.       * Another situation where this occurs, is when we move the interval
  317.       * from [a,b] to [c,b] and then search for a min/max along the new
  318.       * interval, f'(c) is going to zero and we want the next root, not
  319.       * the one we've already found. So we need to walk the start of the
  320.       * interval in this case as well.
  321.       * Walking might be an overkill, it might only be necessary to jump
  322.       * one arbitrary amount(EPS or 2*EPS). I still need to experimentally
  323.       * decide which is more efficient.
  324.       *
  325.       */
  326.      {
  327.     Float y0,y1,c;
  328.     int i,diff_ret;
  329.  
  330.     /* find the next root of f'() and store in c */
  331.     diff_ret = FindRoot(f1,(degree-1),a,b,&c,flag);
  332.  
  333.     /* Evaluate the polynomial at the beginning of the interval
  334.      * and walk it forward if it is zero
  335.          */
  336.     FR_EVALPOLY(f0,degree,a,y0);
  337.     loop=0;
  338.     while( (FRIsZero(y0)) && (loop<=10) )
  339.     {
  340.      a+=FR_EPS_2;
  341.      FR_EVALPOLY(f0,degree,a,y0);
  342.      loop++;
  343.     }
  344.     
  345.  
  346.     /*
  347.      * No roots of ff'() in [a,b] so there is no min/max
  348.      */
  349.     if (!diff_ret)
  350.     {
  351.         /* Evaluate polynomial at point b */
  352.         FR_EVALPOLY(f0,degree,b,y1);
  353.  
  354.         /* if ff(a) and ff(b) have the same sign return false 
  355.          * else use Newton */
  356.         if (FR_SGN(y0) == FR_SGN(y1)) return FALSE;
  357.         return( Newton(f0,f1,degree,a,b,root) );
  358.     }
  359.  
  360.     /*
  361.      * ff'() does have a root at c where  a < c < b.
  362.      */
  363.     FR_EVALPOLY(f0,degree,c,y1);    
  364.  
  365.     /*
  366.      * if same sign move interval to [c,b] and recall FindRoot
  367.      * else there is a root in [a,c] use Newton to solve.
  368.      */
  369.     if (FR_SGN(y0) == FR_SGN(y1))
  370.         return(FindRoot(f0,degree,c,b,root,flag));
  371.     else
  372.         return( Newton(f0,f1,degree,a,c,root) );
  373.       } /* end of degree >= 3 */
  374. } /* end of routine */
  375.  
  376.  
  377. /*
  378.  * This routine numerically tries to find a root of f0() in the interval
  379.  * [a,b].
  380.  */
  381. int Newton(f0,f1,degree,a,b,root)
  382. Float *f0;    /* f0() is the polynomial */
  383. Float *f1;    /* f1() is the 1st derivative of the polynomial */
  384. int degree;     /* degree is the degree of the polynomial f0() */
  385. Float a,b;     /* this is the interval [a,b] to find the roots on */
  386. Float *root;    /* if a root is found, put here */
  387. {
  388.     register int i,loop_num;
  389.     register Float y0,y1,x0;
  390.  
  391.     /* loop an arbitrary number of times, if no root is found then
  392.      * give up. In experiments with cubicspin and blob code, The root
  393.          * was either resolved in <18 iterations or it was never(50+) 
  394.          * resolved.
  395.      */
  396.     loop_num = 50;
  397.     /* do a bisection to start with in case slope is ~0 at a.
  398.      * this is likely since the interval moves to the min/max's
  399.      * of the derivatives.
  400.          */
  401.     x0 = 0.5 * (a + b);  
  402.     do
  403.     {
  404.         FR_EVALPOLY(f0,degree,x0,y0);  /* y0 = f(x0) */ 
  405.         /* check to see if we are close enough to zero to
  406.          * call it a success 
  407.          */
  408.         if ( FRIsZero(y0) ) 
  409.         { 
  410.             /* is it within range? This might be redundant.
  411.              */
  412.             if ( (x0>(a+FR_EPS_2)) && (x0<b) )
  413.             {
  414.                 int ix;
  415.                 *root = x0; 
  416.                 ix = 0; 
  417.                 while(   (gg[degree].valid[ix]==VALID)
  418.                       && (ix<degree)                 ) ix++; 
  419.                 gg[degree].valid[ix] = VALID;
  420.                 gg[degree].roots[ix] = x0; 
  421.                 return TRUE; 
  422.             } 
  423.             else return FALSE;
  424.         }
  425.  
  426.         FR_EVALPOLY(f1,(degree-1),x0,y1);  
  427.         /* 
  428.          * Value of derivative is zero and that's not real good,
  429.          * but it's possible. Revert to bisection method 
  430.          */
  431.         if (FRIsZero3(y1))    x0 = (x0 + b)/2.0; 
  432.         else
  433.         {
  434.             register Float tmp_x;
  435.             tmp_x = x0 - (y0/y1);
  436.  
  437.             /*
  438.              * Newton tunneled out of interval [a,b]. This could 
  439.              * result in skipping over a valid root in the interval
  440.              * and finding a root outside of the interval. 
  441.              * Compromise by going half way between last x0 and 
  442.              * whichever end we jumped outside of. Basically 
  443.              * the bisection method.
  444.               */
  445.             if (tmp_x > b) tmp_x=(x0+b)/2.0; 
  446.             else
  447.             if (tmp_x < a) tmp_x=(x0+a)/2.0;
  448.             x0 = tmp_x; 
  449.         }
  450.         loop_num--;
  451.     } while(loop_num);
  452.     return FALSE;
  453. } /* end of Newton */
  454.  
  455.